﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

namespace PcArduinoControl
{
	/// <summary>
	/// Repesents an Arduino board.
	/// </summary>
	public abstract class Arduino :
		IDisposable
	{
		public event ConnectionFailureHandler ConnectionFailure;

		ICollection<byte> mDigitalIoPinNumbers;
		ICollection<byte> mPwmOutPinNumbers;
		ICollection<byte> mAnalogInPinNumbers;
		IList<RxTxPinNumberPair> mRxTxPinNumberPairs;

		Dictionary<byte, PinType> mDigitalPinTypes;
		Dictionary<byte, DigitalInputPin> mDigitalInputs;
		Dictionary<byte, AnalogInputPin> mAnalogInputs;
		Dictionary<byte, DigitalOutputPin> mDigitalOutputs;
		Dictionary<byte, DigitalAnalogOutputPin> mAnalogOutputs;

		Protocol mProtocol;

		public Log Log { get; private set; }

		// This is the external connected value, we may do a reset internally, but this wouldn't get disconnected unless we can't fix it.
        public bool Connected { get; private set; }
		object mConnectionLock = new object();

		private Watchdog mWatchdog;

		public Arduino(string port)
		{
			Log = new Log();

			mProtocol = new Protocol(port, Log);

			mDigitalIoPinNumbers = new List<byte>();
			mPwmOutPinNumbers = new List<byte>();
			mAnalogInPinNumbers = new List<byte>();
			mRxTxPinNumberPairs = new List<RxTxPinNumberPair>();

			mDigitalPinTypes = new Dictionary<byte, PinType>();
			mDigitalInputs = new Dictionary<byte, DigitalInputPin>();
			mAnalogInputs = new Dictionary<byte, AnalogInputPin>();
			mDigitalOutputs = new Dictionary<byte, DigitalOutputPin>();
			mAnalogOutputs = new Dictionary<byte, DigitalAnalogOutputPin>();

			mWatchdog = new Watchdog(200, 1000, this);

            Connected = false;

			SetupPinSpecifications();
			
			foreach (byte pin in mDigitalIoPinNumbers)
				mDigitalPinTypes.Add(pin, PinType.Unconfigured);

			foreach (byte pin in mAnalogInPinNumbers)
			{
				var input = new AnalogInputPin(pin);
				input.StreamModeChanged += DigitalInputStream;
				mAnalogInputs.Add(input);
			}

			mProtocol.ReceivedMessage += HandleReceivedMessage;
			mProtocol.ProtocolFailure += new ProtocolFailureHandler(HandleProtocolFailure);
			mWatchdog.WatchdogTimeout += HandleWatchdogTimeout;
		}

		public void Dispose()
		{
			Log.Close();
			mWatchdog.Dispose();
			mWatchdog = null;
			mProtocol.Dispose();
			mProtocol = null;
			GC.SuppressFinalize(this);
		}

		/// <summary>
		/// Connect to physical Arduino.
		/// </summary>
		/// <param name="timeoutInSeconds">Must be 2 or greater.</param>
		public void Connect(int timeoutInSeconds = 5)
		{
			if (!ConnectInternal(timeoutInSeconds))
               			 throw new ConnectionFailureException();
                }

		void ConnectSafely(int timeoutInSeconds = 5)
		{
			if (!ConnectInternal(timeoutInSeconds))
				OnConnectionFailure();
		}
		
		bool ConnectInternal(int timeoutInSeconds)
		{
			lock (mConnectionLock)
			{
				if(mProtocol.Status == Protocol.State.Connected)
					return true;
				
				Stop();

				if (timeoutInSeconds < 2)
					throw new ArgumentException("Calling Connect with \"seconds\" parameter less than 2.");

				try
				{
					mProtocol.Open();
				}
				catch (Exception ex)
				{
					Log.WriteException(ex);
					Connected = false;
					return false;
				}

				int connectTrys = 0;

				// Should give the arduino bootloader time to finish
				Thread.Sleep(1000);

				// Connects
				while (true)
				{
					if (connectTrys > (timeoutInSeconds - 1))
					{
						Connected = false;
						return false;
					}

					Log.WriteLine("Arduino: Connecting");
					Exchange connect = mProtocol.SendCommand(new CommandConnect());
					connect.GetFailCode(1000);
					mProtocol.Clear();
					if (connect.FailCode == FailCode.Success || connect.FailCode == FailCode.AlreadyDone)
					{
                        mProtocol.Connected();
						mWatchdog.StartFeeding();
						break;
					}
					Log.WriteLine("Arduino: Failed to Connect");
					Log.WriteCommunicationsLogs();
					connectTrys++;
				}

				SendDigitalPinConfiguration();
				// Set up streaming
				foreach (var pin in mAnalogInputs.Values)
				{
					if (pin.StreamMode != InputStreamMode.Never)
					{
						mProtocol.SendCommand(new CommandInputAnalogRequest(pin.Number));
						mProtocol.SendCommand(new CommandInputAnalogStream(pin.Number, pin.StreamMode));
					}
				}
				foreach (var pin in mDigitalInputs.Values)
				{
					if (pin.StreamMode != InputStreamMode.Never)
					{
						mProtocol.SendCommand(new CommandInputDigitalRequest(pin.Number));
						mProtocol.SendCommand(new CommandInputDigitalStream(pin.Number, pin.StreamMode));
					}
				}
				foreach (var pin in mAnalogOutputs.Values)
				{
					if (pin.IsUsingAnalog)
					{
                        mProtocol.SendCommand(new CommandOutputAnalog(pin.Number, pin.AnalogValue));
					}
				}
				foreach (var pin in mDigitalOutputs.Values)
				{
                    if (pin is DigitalAnalogOutputPin && ((DigitalAnalogOutputPin)pin).IsUsingAnalog == true)
                        continue;

                    mProtocol.SendCommand(new CommandOutputDigital(pin.Number, pin.DigitalValue));
				}

				mWatchdog.StartWatching();

				Log.WriteLine("Arduino: Connected");
				Connected = true;
				return true;
			}
		}

		public void Disconnect()
		{
			mProtocol.SendCommand(new CommandDisconnect());
			Stop();
            Connected = false;
		}

        void Stop()
        {
            mWatchdog.Stop();
            mProtocol.Close();
        }

		public void Wake()
		{
            throw new NotSupportedException(); // There may be a command, but it doesn't do anything
			//mProtocol.SendCommand(new CommandWake());
		}

		public void Sleep()
		{
            throw new NotSupportedException(); // There may be a command, but it doesn't do anything
			//mProtocol.SendCommand(new CommandSleep());
		}

		internal void WatchdogFeed()
		{
			mProtocol.SendCommand(new CommandWatchdogFeed());
		}

		public void EStop()
		{
			Exchange exchange = mProtocol.SendCommand(new CommandEStop());
            Log.WriteLine("Arduino: EStop");

			Stop();
            Connected = false;
		}

		public DigitalInputPin GetDigitalInput(byte pin)
		{
			// Check for sanity in the argument.
			if (!mDigitalIoPinNumbers.Contains(pin))
				throw new NonexistantDigitalIoPinException(pin);
			else if (mDigitalPinTypes[pin] == PinType.Output)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Output);
			else if (mDigitalPinTypes[pin] == PinType.Unconfigured)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Unconfigured);
			else if (mDigitalPinTypes[pin] != PinType.Input)
				throw new IncorrectDigitalIoPinTypeException(pin);
			else
				return mDigitalInputs[pin];
		}

		public AnalogInputPin GetAnalogInput(byte pin)
		{
			// Check for sanity in the argument.
			if (!mAnalogInputs.ContainsKey(pin))
				throw new NonexistantAnalogInputPinException(pin);
			else
				return mAnalogInputs[pin];
		}

		public DigitalOutputPin GetDigitalOutput(byte pin)
		{
			// Check for sanity in the argument.
			if (!mDigitalIoPinNumbers.Contains(pin))
				throw new NonexistantDigitalIoPinException(pin);
			else if (mDigitalPinTypes[pin] == PinType.Input)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Input);
			else if (mDigitalPinTypes[pin] == PinType.Unconfigured)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Unconfigured);
			else if (mDigitalPinTypes[pin] != PinType.Output)
				throw new IncorrectDigitalIoPinTypeException(pin);
			else
				return mDigitalOutputs[pin];
		}

		public DigitalAnalogOutputPin GetAnalogOutput(byte pin)
		{
			// Check for sanity in the argument.
			if (!mDigitalIoPinNumbers.Contains(pin))
				throw new NonexistantDigitalIoPinException(pin);
			else if (!IsDigitalPinPwm(pin))
				throw new NonPwmDigitalOutputPinException(pin);
			else if (mDigitalPinTypes[pin] == PinType.Input)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Input);
			else if (mDigitalPinTypes[pin] == PinType.Unconfigured)
				throw new IncorrectDigitalIoPinTypeException(pin, PinType.Unconfigured);
			else if (mDigitalPinTypes[pin] != PinType.Output)
				throw new IncorrectDigitalIoPinTypeException(pin);
			else
				return mAnalogOutputs[pin];
		}

		public void ConfigureDigitalPins(params PinType[] pinTypes)
		{
			for (byte index = 0; index < pinTypes.Length; index++)
				ConfigureDigitalPin(index, pinTypes[index]);

            if(Connected)
                SendDigitalPinConfiguration(); // else it will be sent as soon as we connect
		}

        void SendDigitalPinConfiguration()
        {
            int len = mDigitalPinTypes.Keys.Max();
            PinType[] types = new PinType[len];
            for (byte i = 0; i < len; i++)
            {
                if (mDigitalPinTypes.ContainsKey(i))
                    types[i] = mDigitalPinTypes[i];
                else
                    types[i] = PinType.Unconfigured;
            }

            mProtocol.SendCommand(new CommandConfigPins(types));
        }

		protected abstract void SetupPinSpecifications();

		protected void AddDigitalIoPinNumbers(params byte[] pins)
		{
			foreach (byte pin in pins)
				mDigitalIoPinNumbers.Add(pin);
		}

		protected void AddPwmOutPinNumbers(params byte[] pins)
		{
			foreach (byte pin in pins)
				mPwmOutPinNumbers.Add(pin);
		}

		protected void AddAnalogInPinNumbers(params byte[] pins)
		{
			foreach (byte pin in pins)
				mAnalogInPinNumbers.Add(pin);
		}

		protected void AddRxTxPinNumberPair(byte rxPin, byte txPin)
		{
			mRxTxPinNumberPairs.Add(new RxTxPinNumberPair(rxPin, txPin));
		}

		bool IsDigitalPinInput(byte pin)
		{
			return (mDigitalPinTypes[pin] == PinType.Input);
		}

		bool IsDigitalPinOutput(byte pin)
		{
			return (mDigitalPinTypes[pin] == PinType.Output);
		}

		bool IsDigitalPinPwm(byte pin)
		{
			return (mPwmOutPinNumbers.Contains(pin));
		}

		/// <summary>
		/// Configures the specified digital pin to the specified type.
		/// </summary>
		/// <param name="pin">Digital Pin Number</param>
		/// <param name="type">Input or Output</param>
		/// <returns>If the pin is already configured as specified.</returns>
		void ConfigureDigitalPin(byte pin, PinType type)
		{
			// Check for sanity in arguments.
			if (!mDigitalIoPinNumbers.Contains(pin))
				throw new NonexistantDigitalIoPinException(pin);

			var typePrevious = mDigitalPinTypes[pin];
			if (mDigitalPinTypes[pin] == type)
				return;
			else
			{
				switch (typePrevious)
				{
					case PinType.Input:
						mDigitalInputs.Remove(pin);
						break;
					case PinType.Output:
						if (IsDigitalPinPwm(pin))
							mAnalogOutputs.Remove(pin);
						mDigitalOutputs.Remove(pin);
						break;
					case PinType.Unconfigured:
						// Don't need to do anything.
						break;
				}

				switch (type)
				{
					case PinType.Input:
						// Configure to be a Digital Input.
						mDigitalPinTypes[pin] = PinType.Input;
						var input = new DigitalInputPin(pin);
						input.StreamModeChanged += DigitalInputStream;
						mDigitalInputs.Add(input);
						break;
					case PinType.Output:
						// Configure to be a Digital Output.
						mDigitalPinTypes[pin] = PinType.Output;
						if (IsDigitalPinPwm(pin))
						{
							var output = new DigitalAnalogOutputPin(pin);
							output.DigitalValueChanged += DigitalOutput;
							output.AnalogValueChanged += AnalogOutput;
							mDigitalOutputs.Add(output);
							mAnalogOutputs.Add(output);
						}
						else
						{
							var output = new DigitalOutputPin(pin);
							output.DigitalValueChanged += DigitalOutput;
							mDigitalOutputs.Add(output);
						}
						break;
					default:
						// Unconfigure the pin.
						mDigitalPinTypes[pin] = PinType.Unconfigured;
						break;
				}

				return;
			}
		}

		// Event Handling Junk-Type Stuff

		void HandleReceivedMessage(Message message)
		{
			switch (message.Type)
			{
				case MessageType.Response:
					ResponseMessage response = (ResponseMessage)message;
					break;
				case MessageType.Notification:
					NotificationMessage notification = (NotificationMessage)message;

					Log.WriteLine("Protocol: Notification Received - " + notification.NotificationCode);

					switch (notification.NotificationCode)
					{
						case NotificationCode.EStop:
							Log.WriteLine("Arduino: EStopped!");
							// TODO: Do something.
							break;
						case NotificationCode.Checksum:
							Log.WriteLine("Arduino: There was a checksum error.");
							break;
						case NotificationCode.Restarting:
                            Stop();
							ConnectSafely();
							break;
						default:
							// TODO: Do something.
							break;
					}

					break;
				case MessageType.DigitalInput:
					DigitalInputMessage digitalInput = (DigitalInputMessage)message;
					GetDigitalInput(digitalInput.Pin).DigitalValue = digitalInput.Value;
					break;
				case MessageType.AnalogInput:
					AnalogInputMessage analogInput = (AnalogInputMessage)message;
					GetAnalogInput(analogInput.Pin).AnalogValue = analogInput.Value;
					break;
				case MessageType.WatchdogFeed:
					mWatchdog.Feed();
					break;
				case MessageType.ResendRequest:
					Log.WriteLine("Arduino: Requested a resend.");
					break;
				default:
					// TODO: Do something.
					break;
			}
		}

		void HandleProtocolFailure()
		{
            Stop();
			ConnectSafely();
		}

		protected void HandleWatchdogTimeout()
		{
            Stop();
            ConnectSafely();
		}

		void DigitalOutput(byte pin, DigitalValue value)
		{
            if (!Connected)
                throw new NotConnectedException();

			mProtocol.SendCommand(new CommandOutputDigital(pin, value));
		}

		void AnalogOutput(byte pin, byte value)
		{
            if (!Connected)
                throw new NotConnectedException();

			mProtocol.SendCommand(new CommandOutputAnalog(pin, value));
		}

		void DigitalInputStream(byte pin, InputStreamMode mode)
		{
            if (!Connected)
                throw new NotConnectedException();

			// Get the starting point (especially important if mode is onchange).
			mProtocol.SendCommand(new CommandInputDigitalRequest(pin));

			mProtocol.SendCommand(new CommandInputDigitalStream(pin, mode));
		}

		void AnalogInputStream(byte pin, InputStreamMode mode)
		{
            if (!Connected)
                throw new NotConnectedException();

			// Get the starting point (especially important if mode is onchange).
			mProtocol.SendCommand(new CommandInputAnalogRequest(pin));
			
			mProtocol.SendCommand(new CommandInputAnalogStream(pin, mode));
		}

		void DigitalInput(byte pin, DigitalValue value)
		{
			if (mDigitalInputs.ContainsKey(pin))
				mDigitalInputs[pin].DigitalValue = value;
			else
				Log.WriteLine(String.Format("Error: Nonexistant Digital Input Pin {0} from Arduino", pin.ToString()));
		}

		void AnalogInput(byte pin, byte value)
		{
			if (mAnalogInputs.ContainsKey(pin))
				mAnalogInputs[pin].AnalogValue = value;
			else
				Log.WriteLine(String.Format("Error: Nonexistant Analog Input Pin {0} from Arduino", pin.ToString()));
		}

		void OnConnectionFailure()
		{
			var connectionFailure = ConnectionFailure;

			if (connectionFailure != null)
				connectionFailure.Invoke();
		}
	}
}
